package it.unibo.refolding.pico;

import it.unibo.refolding.common.CommonThings;
import it.unibo.refolding.common.LinearFunction;
import it.unibo.refolding.common.MyRuntimeException;
import it.unibo.refolding.common.Sleeper;
import it.unibo.refolding.pico.PicoDataAnalyser.ContactPointAndSlope;

import java.awt.Point;
import java.awt.geom.Point2D;
import java.io.IOException;
import java.io.OutputStream;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.nio.file.StandardOpenOption;
import java.nio.file.attribute.BasicFileAttributeView;
import java.nio.file.attribute.FileTime;
import java.util.Arrays;
import java.util.Iterator;
import java.util.List;
import java.util.Locale;
import java.util.PriorityQueue;
import java.util.Queue;
import java.util.Random;
import java.util.Set;
import java.util.concurrent.ThreadLocalRandom;
import java.util.logging.Level;
import java.util.logging.Logger;

import org.apache.commons.cli.CommandLine;
import org.apache.commons.cli.GnuParser;
import org.apache.commons.cli.Option;
import org.apache.commons.cli.Options;
import org.apache.commons.cli.ParseException;
import org.apache.commons.cli.Parser;
import org.apache.commons.lang3.StringUtils;
import org.apache.commons.lang3.builder.CompareToBuilder;
import org.apache.commons.lang3.builder.EqualsBuilder;
import org.apache.commons.lang3.builder.HashCodeBuilder;
import org.apache.commons.lang3.builder.ReflectionToStringBuilder;
import org.apache.commons.lang3.builder.ToStringStyle;
import org.apache.commons.math3.linear.Array2DRowRealMatrix;
import org.apache.commons.math3.linear.ArrayRealVector;
import org.apache.commons.math3.linear.DecompositionSolver;
import org.apache.commons.math3.linear.LUDecomposition;
import org.apache.commons.math3.linear.RealVector;
import org.apache.commons.math3.linear.SingularMatrixException;

import com.google.common.base.Preconditions;
import com.google.common.collect.ImmutableList;
import com.google.common.collect.Sets;

public class ApproachRetractionAssistant {

  private static final Logger logger = Logger.getLogger(ApproachRetractionAssistant.class.getSimpleName());

  public static class TotalTimeVelIndex implements Comparable<TotalTimeVelIndex> {

    private final long totalTime;
    public final int velIndex;

    public TotalTimeVelIndex(long totalTime, int velIndex) {
      this.totalTime = totalTime;
      this.velIndex = velIndex;
    }

    @Override
    public int compareTo(TotalTimeVelIndex other) {
      return new CompareToBuilder().append(totalTime, other.totalTime).append(
        velIndex, other.velIndex).toComparison();
    }

    @Override
    public boolean equals(Object other) {
      return EqualsBuilder.reflectionEquals(this, other);
    }

    @Override
    public int hashCode() {
      return HashCodeBuilder.reflectionHashCode(this);
    }

    @Override
    public String toString() {
      return ReflectionToStringBuilder.toString(this, ToStringStyle.SHORT_PREFIX_STYLE);
    }
  }

  public static void main(String[] args) throws InterruptedException, ParseException, IOException {
    Options options = new Options();
    addDirToWatchOption(options);
    addSleepTimeOption(options);

    Option nanoDriverHostOption = NanoDriverUtil.createNanoDriverHostOption();
    options.addOption(nanoDriverHostOption);
    Option nanoDriverPortOption = NanoDriverUtil.createNanoDriverPortOption();
    options.addOption(nanoDriverPortOption);

    Option nanoDriverClassOption = NanoDriverUtil.createNanoDriverClassOption();
    options.addOption(nanoDriverClassOption);

    Option rangeStartPnOption = new Option(
      "rangeStartPn", true, "Lower bound of allowed pushing force in pN.");
    rangeStartPnOption.setRequired(true);
    rangeStartPnOption.setType(Double.TYPE);
    options.addOption(rangeStartPnOption);

    Option rangeEndPnOption = new Option(
      "rangeEndPn", true, "Upper bound of allowed pushing force in pN.");
    rangeEndPnOption.setRequired(true);
    rangeEndPnOption.setType(Double.TYPE);
    options.addOption(rangeEndPnOption);

    Option forceRecordOption = new Option(
      "forceRecord", true, "Record the screen location even if already found in preferences..");
    forceRecordOption.setRequired(true);
    forceRecordOption.setType(Double.TYPE);
    options.addOption(forceRecordOption);

    Option equalTimeOption = createEqualTimeOption();
    options.addOption(equalTimeOption);

    Option reverseVelocitiesNmPerSOption = createReverseVelocitiesNmPerSOption();
    options.addOption(reverseVelocitiesNmPerSOption);

    Option deflectionVoltagePathOption = new Option(
      "deflectionVoltagePath", true, "where to write the min and max voltages");
    deflectionVoltagePathOption.setRequired(true);
    deflectionVoltagePathOption.setType(String.class);
    options.addOption(deflectionVoltagePathOption);

    Option xOffsetStdDevNmOption = PicoUtil.createXOffsetStdDevNmOption();
    options.addOption(xOffsetStdDevNmOption);

    Option xOffsetChangeFrequencyOption = PicoUtil.createXOffsetChangeFrequencyOption();
    options.addOption(xOffsetChangeFrequencyOption);

    Option pausePathOption = new Option(
      "pausePath", true, "When this file exists, it stops until it is deleted.");
    options.addOption(pausePathOption);

    Option pauseMillisOption = new Option(
      "pauseMillis", true,
      "How much to wait before checking again to see if pausePath still exists.");
    options.addOption(pauseMillisOption);

    Parser parser = new GnuParser();
    CommandLine line = parser.parse(options, args);
    logger.log(Level.INFO, "args={0}", Arrays.asList(args));

    String nanoDriverClassname = line.getOptionValue(nanoDriverClassOption.getOpt());
    if (PicoUtil.isFakeOrRemoteNanoDriver(nanoDriverClassname)) {
      System.setProperty("java.awt.headless", "true");
    }
    String nanoDriverHost = line.getOptionValue(nanoDriverHostOption.getOpt());
    int nanoDriverPort = Integer.parseInt(line.getOptionValue(nanoDriverPortOption.getOpt()));
    Path pausePath = Paths.get(StringUtils.trim(line.getOptionValue(pausePathOption.getOpt())));
    long pauseMillis = Long.parseLong(line.getOptionValue(pauseMillisOption.getOpt()));
    String dirToWatch = StringUtils.trim(line.getOptionValue("dirToWatch"));
    String deflectionVoltagePath = StringUtils.trim(line.getOptionValue(deflectionVoltagePathOption.getOpt()));
    long sleepTimeMs = Long.parseLong(line.getOptionValue("sleepTimeMs", "500"));
    double rangeStart = Double.parseDouble(line.getOptionValue(rangeStartPnOption.getOpt()));
    double rangeEnd = Double.parseDouble(line.getOptionValue(rangeEndPnOption.getOpt()));
    double xOffsetStdDevM = Double.parseDouble(line.getOptionValue(xOffsetStdDevNmOption.getOpt())) / 1e9;
    long xOffsetChangeFrequency = Long.parseLong(line.getOptionValue(xOffsetChangeFrequencyOption.getOpt()));
    boolean equalTime = Boolean.parseBoolean(line.getOptionValue(equalTimeOption.getOpt()).toLowerCase());
    String revVelsParam = line.getOptionValue(reverseVelocitiesNmPerSOption.getOpt());
    double[] revVels = parseReverseVelocities(revVelsParam);
    Preconditions.checkArgument(
      rangeStart <= rangeEnd, "rangeStart must be less than or equal to rangeEnd");

    final NanoDriverConfig config;
    if (PicoUtil.isFakeOrRemoteNanoDriver(nanoDriverClassname)) {
      config = null;
    } else {
      config = new NanoDriverConfig();
      boolean forceRecord = Boolean.parseBoolean(line.getOptionValue(forceRecordOption.getOpt()).toLowerCase());
      List<Point> points = NanoDriverUtil.recordOrFetchScreenLocations(
        ImmutableList.of("ZScanStart", "Reverse Velocity", "X offset", "Continuous"), forceRecord);
      config.zScanStartLocation = points.get(0);
      config.reverseVelocityLocation = points.get(1);
      config.xOffsetLocation = points.get(2);
      config.continuousLocation = points.get(3);
    }

    NanoDriver nanoDriver = PicoUtil.nanoDriver(
      nanoDriverClassname, config, nanoDriverHost, nanoDriverPort);
    Path dir = Paths.get(dirToWatch);
    logger.log(Level.INFO, "Watching directory {0}.", dir.toAbsolutePath());
    // Doing it twice at the start for now.
    Set<Path> seen = Sets.newHashSet(CommonThings.listDir(dir)); 
    long[] totalNanos = new long[revVels.length];
    Queue<TotalTimeVelIndex> pq = (equalTime && revVels.length != 0) ? new PriorityQueue<TotalTimeVelIndex>()
      : null;
    for (int i = 0; i < totalNanos.length; i++) {
      if (pq != null) {
        pq.add(new TotalTimeVelIndex(totalNanos[i], i));
      }
    }
    int velIndex = revVels.length == 0 ? -1 : (equalTime ? pq.remove().velIndex : 0);
    Boolean expectedNm = null;
    Random rnd = ThreadLocalRandom.current();
    long parsedFileIndex = 0;
    long latestNanoTime = System.nanoTime();
    Level level = Level.FINE;
    try (OutputStream deflectionVoltageOutputStream = StringUtils.isEmpty(deflectionVoltagePath) ? null
      : Files.newOutputStream(
        Paths.get(deflectionVoltagePath), StandardOpenOption.CREATE, StandardOpenOption.APPEND)) {
      while (true) {
        if (pausePath.toString().length() != 0) {
          while (Files.exists(pausePath)) {
            logger.log(Level.INFO, "Sleeping {0} ms because {1} exists.", new Object[] {
            pauseMillis, pausePath});
            Thread.sleep(pauseMillis);
          }
        }
        Set<Path> current = Sets.newHashSet(CommonThings.listDir(dir));
        // Do not trust the time.
        Set<Path> diff = Sets.difference(current, seen);
        Path latestFile = latestFile(dir, diff);
        if (latestFile == null) {
          assert diff.isEmpty();
          if (xOffsetStdDevM != 0) {
            // Changing x often stops the experiment.
            nanoDriver.continuous();
          }
          Thread.sleep(sleepTimeMs);
        } else {
          assert !diff.isEmpty();
          logger.log(Level.INFO, "New files: {0} Latest file: {1}.", new Object[] {
          diff.size(), latestFile.getFileName()});
          if (logger.isLoggable(level)) {
            logger.log(level, "Time status: {0}", timeStatus(revVels, totalNanos));
          }
          seen = current;
          long timeToWaitForSecondReading = (sleepTimeMs + 1) / 2;
          byte[] fileContent = CommonThings.readToByteArrayWithRetryOrNull(
            latestFile, timeToWaitForSecondReading, 2, Sleeper.SYSTEM_SLEEPER);
          if (fileContent != null) {
            try {
              PlainPicoData picoData = (PlainPicoData) PicoDataExtractor.parse(fileContent);
              if (picoData == null) {
                logger.log(Level.WARNING, "Could not extract data from {0}.", latestFile);
              } else {
                short[] minMaxDeflection = picoData.minMaxDeflection();
                // See PicoDataExtractor deflectionLsbToMetresFactor
                // calculation.
                double minDeflectionVoltage = picoData.deflectionLimitV * minMaxDeflection[0] / (double) 0xFFFF;
                double maxDeflectionVoltage = picoData.deflectionLimitV * minMaxDeflection[1] / (double) 0xFFFF;
                logger.log(
                  Level.FINE, "minDeflectionVoltage={0} V maxDeflectionVoltage={1} V",
                  new Object[] {minDeflectionVoltage, maxDeflectionVoltage});
                if (deflectionVoltageOutputStream != null) {
                  byte[] bytes = String.format(
                    "%s,%s,%s\n", latestFile, Double.toString(minDeflectionVoltage),
                    Double.toString(maxDeflectionVoltage)).getBytes("ISO-8859-1");
                  deflectionVoltageOutputStream.write(bytes);
                  deflectionVoltageOutputStream.flush();
                }
                if (velIndex != -1) {
                  logger.log(
                    Level.INFO, "Latest retraction velocity: {0} nm/s",
                    picoData.retraction.velocity * 1e9);
                  if (pq != null) {
                    Preconditions.checkState(pq.size() == revVels.length - 1);
                  }
                  if (expectedNm == null
                    || expectedNm.booleanValue() == picoData.retraction.velocity < 1e-6) {
                    long t = System.nanoTime();
                    int newVelIndex = updateTime(totalNanos, pq, velIndex, latestNanoTime, t);
                    latestNanoTime = t;
                    double revVelMPerS = revVels[newVelIndex];
                    // Optimisation: only change velocity if needed. Relative
                    // error for 50.1 < 2.3e-3.
                    if (velIndex != newVelIndex
                      || Math.abs(picoData.retraction.velocity / revVelMPerS - 1) > 5e-3) {
                      final double value;
                      if (picoData.retraction.velocity < 1e-6) {
                        value = revVelMPerS * 1e9;
                      } else {
                        value = revVelMPerS * 1e6;
                      }
                      nanoDriver.reverseVelocityNmOrUmPerS(value);
                    }
                    expectedNm = revVelMPerS < 1e-6;
                    velIndex = newVelIndex;
                  } else {
                    logger.log(Level.WARNING, "expectedNm={0} actualRevVel={1} nm/s", new Object[] {
                    expectedNm, picoData.retraction.velocity * 1e9});
                  }
                }
                if (xOffsetStdDevM != 0 && parsedFileIndex % xOffsetChangeFrequency == 0) {
                  double oldXOffset = picoData.xOffsetM;
                  double xOffsetM = PicoUtil.generateXOffset(oldXOffset, xOffsetStdDevM, rnd);
                  nanoDriver.xOffsetM(xOffsetM);
                }
                parsedFileIndex += 1;
              }
              actOnApproachMetresOnly(rangeStart / 1e12 / picoData.springConstantNPerM, rangeEnd
                / 1e12 / picoData.springConstantNPerM, picoData, nanoDriver);
            } catch (Throwable t) {
              CommonThings.writeToErrFile(t, latestFile);
              System.err.println(t);
              logger.severe(t.getMessage());
            }
          }
        }
      }
    }
  }

  public static int updateTime(
    long[] totalNanos, Queue<TotalTimeVelIndex> pq, int index, long latestNanoTime,
    long currentNanoTime) {
    if (pq != null) {
      Preconditions.checkArgument(totalNanos.length == pq.size() + 1);
    }
    totalNanos[index] += currentNanoTime - latestNanoTime;
    final int newVelIndex;
    if (pq != null) {
      pq.add(new TotalTimeVelIndex(totalNanos[index], index));
      newVelIndex = pq.remove().velIndex;
      // The velocity index may remain the same.
    } else {
      newVelIndex = (index + 1) % totalNanos.length;
    }
    return newVelIndex;
  }

  static double[] parseReverseVelocities(String revVelsParam) {
    String[] revVelsNmPerS = revVelsParam.isEmpty() ? new String[0]
      : revVelsParam.split("\\s*,\\s*");
    double[] revVels = new double[revVelsNmPerS.length];
    for (int i = revVels.length; --i >= 0;) {
      revVels[i] = Double.parseDouble(revVelsNmPerS[i]) / 1e9;
    }
    return revVels;
  }

  static Option createReverseVelocitiesNmPerSOption() {
    Option reverseVelocitiesNmPerSOption = new Option(
      "reverseVelocitiesNmPerS", true,
      "comma-separated list of velocities_nm_per_s. If empty, reverse velocities will not be touched.");
    reverseVelocitiesNmPerSOption.setRequired(true);
    reverseVelocitiesNmPerSOption.setType(String.class);
    return reverseVelocitiesNmPerSOption;
  }

  static Option createEqualTimeOption() {
    Option equalTimeOption = new Option(
      "equalTime", true, "Record the screen location even if already found in preferences..");
    equalTimeOption.setRequired(true);
    equalTimeOption.setType(Double.TYPE);
    return equalTimeOption;
  }

  public static String timeStatus(double[] revVels, long[] totalNanos) {
    Preconditions.checkArgument(revVels.length == totalNanos.length);
    String[] a = new String[totalNanos.length];
    for (int i = 0; i < totalNanos.length; i++) {
      a[i] = String.format(Locale.ROOT, "%s:%s", revVels[i] * 1e9, totalNanos[i] / 1e9);
    }
    return "(nms/s:s) " + Arrays.toString(a);
  }

  static void addSleepTimeOption(Options options) {
    Option sleepTimeOption = new Option(
      "sleepTimeMs", true, "how long to sleep when there are no new files");
    sleepTimeOption.setRequired(true);
    sleepTimeOption.setType(Long.TYPE);
    options.addOption(sleepTimeOption);
  }

  static void addDirToWatchOption(Options options) {
    Option dirToWatchOption = new Option(
      "dirToWatch", true, "filter only curves with at least this amount of peaks");
    dirToWatchOption.setRequired(true);
    dirToWatchOption.setType(String.class);
    options.addOption(dirToWatchOption);
  }

  private static void actOnApproachMetresOnly(
    double rangeStart, double rangeEnd, PlainPicoData picoData, NanoDriver nanoDriver) {
    ContactPointAndSlope approachIndexAndZLsbDefLsbSlopes = PicoDataAnalyser.findApproachContactPointAndSlope(
      picoData.sharedApproach(), Double.NaN);
    double pushingDistanceM = approachPushingDistanceZLsb(
      picoData.approach, approachIndexAndZLsbDefLsbSlopes) * picoData.deflectionLsbToMetresFactor();
    moveAsNeeded(pushingDistanceM, picoData, rangeStart, rangeEnd, nanoDriver);
  }

  public static double approachPushingDistanceZLsb(
    DirectionData approach, ContactPointAndSlope approachContactPointAndSlope) {
    int i = approach.timeInstantCount();
    while (--i >= 0 && approach.deflectionAtIndex(i) < approachContactPointAndSlope.defLsb) {
    }
    int argMax = 0;
    for (int j = 1; j <= i; ++j) {
      if (approach.deflectionAtIndex(j) > approach.deflectionAtIndex(argMax)) {
        argMax = j;
      }
    }
    if (argMax > i / 20) {
      logger.warning(String.format(
        Locale.ROOT, "The maximum should be very close to point zero but it is instead argMax=%s",
        argMax));
    }
    /*
     * It turns out that looking at the deflection data is much more accurate
     * than looking just at the fraction of the ramp size, or even actual Z
     * range, travelled by the piezo during time from argMax to
     * approachContactPointIndex. For example the difference can be between 1.3
     * V in the first case and 0.9, or 0.99 in the second one. Explanation:
     * Things are not linear on the whole range, and that's why the Z range
     * approach does not work well. The deflection approach works because the
     * delta that we compute making the linear assumption will in fact change
     * the curve, by that delta, in the linear regime. That is, the non-linear
     * part will stay the same, only the linear one farther from the surface
     * will increase/decrease! The delta between the 2 curves will indeed be
     * perfectly linear.
     */
    double pushingLsb = approach.deflectionAtIndex(argMax) - approachContactPointAndSlope.defLsb;
    Preconditions.checkState(
      pushingLsb >= 0, "max=%s cpDefLsb=%s", approach.deflectionAtIndex(argMax),
      approachContactPointAndSlope.defLsb);
    return pushingLsb;
  }

  private static void moveAsNeeded(
    double pushingDistanceM, PicoData picoData, double rangeStartM, double rangeEndM,
    NanoDriver nanoDriver) {
    if (pushingDistanceM < rangeStartM | pushingDistanceM > rangeEndM) {
      double deltaM = (rangeStartM + rangeEndM) / 2 - pushingDistanceM;
      logger.log(
        Level.INFO,
        "pushingDistance={0} nm is outside range [{1}, {2}] nm. Changing ZScan by {3} nm.",
        new Object[] {pushingDistanceM * 1e9, rangeStartM * 1e9, rangeEndM * 1e9, deltaM * 1e9});
      double newZScanM = picoData.zScanStartMetres() + deltaM;
      nanoDriver.zScanStartM(newZScanM);
    } else {
      logger.log(Level.FINE, "pushingForce={0} pN is ok.", pushingDistanceM * 1e12);
    }
  }

  private static Path latestFile(Path parent, Set<Path> difference) {
    // Only consider the latest file.
    Iterator<Path> it = difference.iterator();
    if (!it.hasNext()) {
      return null;
    }
    Path latestFile = parent.resolve(it.next());
    if (it.hasNext()) {
      // avoid redundant lastModified() calls
      FileTime latestFileLastModified;
      try {
        latestFileLastModified = Files.getFileAttributeView(
          latestFile, BasicFileAttributeView.class).readAttributes().lastModifiedTime();
      } catch (IOException e) {
        throw new MyRuntimeException(e.getMessage(), e);
      }
      do {
        Path f = parent.resolve(it.next());
        FileTime lastModified;
        try {
          lastModified = Files.getFileAttributeView(f, BasicFileAttributeView.class).readAttributes().lastModifiedTime();
        } catch (IOException e) {
          throw new MyRuntimeException(e.getMessage(), e);
        }
        if (lastModified.compareTo(latestFileLastModified) > 0) {
          latestFile = f;
          latestFileLastModified = lastModified;
        }
      } while (it.hasNext());
    }
    return latestFile;
  }

  public static class ContactAndFreeLine {
    public final LinearFunction contactLine, freeLine;

    public ContactAndFreeLine(LinearFunction contactLine, LinearFunction freeLine) {
      this.contactLine = contactLine;
      this.freeLine = freeLine;
    }

    @Override
    public String toString() {
      return ReflectionToStringBuilder.toString(this, ToStringStyle.SHORT_PREFIX_STYLE);
    }

    @Override
    public int hashCode() {
      return new HashCodeBuilder().append(contactLine).append(freeLine).toHashCode();
    }

    @Override
    public boolean equals(Object obj) {
      if (!(obj instanceof ContactAndFreeLine)) {
        return false;
      }
      ContactAndFreeLine other = (ContactAndFreeLine) obj;
      return contactLine.equals(other.contactLine) && freeLine == other.freeLine;
    }

    /**
     * @return {@literal null} when the lines coincide or are parallel
     */
    public Point2D.Double contactPoint() {
      Array2DRowRealMatrix rm = new Array2DRowRealMatrix(new double[][] {
      {contactLine.slope, -1}, {freeLine.slope, -1}});
      DecompositionSolver solver = new LUDecomposition(rm).getSolver();
      RealVector solution;
      try {
        solution = solver.solve(new ArrayRealVector(new double[] {
        -contactLine.intercept, -freeLine.intercept}));
      } catch (SingularMatrixException e) {
        logger.log(Level.WARNING, "Could not find contact point: " + e, e);
        return null;
      }
      return new Point2D.Double(solution.getEntry(0), solution.getEntry(1));
    }
  }

  /**
   * Must not throw any exceptions.
   * 
   * @param a
   * @return
   */
  static ContactAndFreeLine findContactAndFreeLine(short[] a) {
    int len = a.length;
    if (len == 0) {
      logger.warning("No data.");
      return null;
    }
    LinearRegression contactLine = findChunkLinearFitWhileRmsdIsImproving(
      a, chunkSize(len, 40), false, 1.2);
    logger.log(Level.FINE, "contactLine={0}", contactLine);
    if (contactLine == null) {
      return null;
    }
    LinearRegression freeLine = findChunkLinearFitWhileRmsdIsImproving(
      a, chunkSize(len, 20), true, 1.5);
    logger.log(Level.FINE, "freeLine={0}", freeLine);
    if (freeLine == null) {
      return null;
    }
    return new ContactAndFreeLine(contactLine.f, freeLine.f);
  }

  private static int chunkSize(int len, final int chunkCount) {
    int chunkSize = (len + chunkCount - 1) / chunkCount;
    return chunkSize;
  }

  static LinearRegression findChunkLinearFitWhileRmsdIsImproving(
    short[] a, int chunkSize, boolean backwards, double factor) {
    Preconditions.checkArgument(factor >= 1);
    LinearRegression best = null;
    LinearRegression latest = null;
    int latestI = -1;
    // At 8720 nms/s at the beginning there is a flat region that doesn't
    // make any sense.
    int left_offset = (a.length + 49) / 50;
    for (int j = backwards ? 0 : left_offset; j < (backwards ? a.length - left_offset : a.length); j += chunkSize) {
      int i;
      if (backwards) {
        i = Math.max(0, a.length - chunkSize - j);
      } else {
        i = j;
      }
      int end = Math.min(a.length, i + chunkSize);
      logger.log(Level.FINE, "Checking chunk [{0},{1}].", new Object[] {i, end});
      LinearRegression lr = LinearRegression.linearRegression(a, i, end);
      if (best == null) {
        best = latest = lr;
        latestI = i;
      } else {
        if (lr == null) {
          break;
        }
        if (best.rmsd > lr.rmsd) {
          best = latest = lr;
          latestI = i;
        } else {
          if (best.rmsd * factor < lr.rmsd) {
            // Started getting worse fits.
            break;
          } else {
            latest = lr;
            latestI = i;
          }
        }
      }
    }
    // Include points to the right until best.rmsd * factor.
    if (latest == null) {
      return null;
    }
    int latestJ = Math.min(latestI + chunkSize, a.length);
    for (int j = latestI + chunkSize + 1; j <= a.length; j += (chunkSize + 1) / 2) {
      LinearRegression lr = LinearRegression.linearRegression(a, latestI, j);
      if (lr == null) {
        break;
      }
      if (best.rmsd * factor < lr.rmsd) {
        break;
      } else {
        latest = lr;
        latestJ = j;
      }
    }
    // Similar to the left.
    for (int i = latestI; (i -= (chunkSize + 1) / 2) >= left_offset;) {
      LinearRegression lr = LinearRegression.linearRegression(a, i, latestJ);
      if (lr == null) {
        break;
      }
      if (best.rmsd * factor < lr.rmsd) {
        break;
      } else {
        latest = lr;
      }
    }
    return latest;
  }
}
